function u = mpc_control_step(mpc_controller, x_current, x_ref, params)
%MPC_CONTROL_STEP 执行MPC单步控制
%
% 输入:
%   mpc_controller - MPC控制器对象
%   x_current - 当前状态 [x; y; theta; v] (4x1)
%   x_ref - 参考状态 [x; y; theta; v] (4x1)
%   params - 参数结构体
%
% 输出:
%   u - 控制输入 [a; delta] (2x1)

    % 简化的MPC控制实现
    % 由于完整的MPC Toolbox可能不可用，这里提供一个基于优化的简化版本
    
    % 如果有MPC Toolbox，可以使用：
    % u = mpcmove(mpc_controller, x_current, x_ref);
    
    % 简化版本: 比例控制 + 前馈
    % 位置误差
    e_x = x_ref(1) - x_current(1);
    e_y = x_ref(2) - x_current(2);
    e_theta = atan2(sin(x_ref(3) - x_current(3)), cos(x_ref(3) - x_current(3)));
    e_v = x_ref(4) - x_current(4);
    
    % 控制增益
    Kp_v = 0.5;
    Kp_theta = 2.0;
    
    % 加速度控制
    a = Kp_v * e_v;
    
    % 转角控制 (Stanley controller风格)
    theta_d = atan2(e_y, e_x);
    e_heading = atan2(sin(theta_d - x_current(3)), cos(theta_d - x_current(3)));
    
    % 横向误差
    e_crosstrack = -sin(x_current(3)) * e_x + cos(x_current(3)) * e_y;
    
    delta = e_heading + atan(Kp_theta * e_crosstrack / (x_current(4) + 0.1));
    
    % 组合控制输入
    u = [a; delta];
    
    % 限幅
    u(1) = max(params.a_min, min(params.a_max, u(1)));
    u(2) = max(params.delta_min, min(params.delta_max, u(2)));
end